/*
 * Copyright (c) 2006-2021, RT-Thread Development Team
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Change Logs:
 * Date           Author       Notes
 * 2023-11-14     ganze       the first version
 */

/*
 * 关于 rt_hw_sensor_register中flag参数与后续工作模式的说明

 * 在sensor.c的rt_hw_sensor_register函数设置设备dev（传感器）的flag
 * 在sensor.c的rt_sensor_open函数，对打开标签oflag进行判断：
 *
 * 假如 oflag & RT_DEVICE_FLAG_RDONLY && dev->flag & RT_DEVICE_FLAG_RDONLY成立
 * 则RT-Thread将传感器工作状态置为轮询模式 ---- RT_SENSOR_MODE_POLLING
 *
 * 假如 oflag & RT_DEVICE_FLAG_INT_RX && dev->flag & RT_DEVICE_FLAG_INT_RX成立
 * 则RT-Thread将传感器工作状态置为中断模式 ---- RT_SENSOR_MODE_INT
 *
 * 假如 oflag & RT_DEVICE_FLAG_FIFO_RX && dev->flag & RT_DEVICE_FLAG_FIFO_RX成立
 * 则RT-Thread将传感器工作状态置为中断FIFO模式 ---- RT_SENSOR_MODE_FIFO
 *
 * 对于以上3种模式，sensor.c在 rt_sensor_cb 做了区分，具体逻辑如下：
 *
 * 1）传感器本身触发中断，发出中断信号
 * 2）MCU绑定信号引脚检测中断，调用irq_callback函数
 * 3）irq_callback函数调用rt_sensor_cb函数
 * 4）rt_sensor_cb函数首先判断sen->parent.rx_indicate == RT_NULL如果成立则结束
 * 5）然后sen->irq_handle != RT_NULL成立则调用sen->irq_handle(sen)，此为用户设置的中断回调函数
 * 6）如果sen->data_len > 0则调用rx_indicate读取所有data_buf数据
 * 7）如果传感器工作状态置为中断模式，则调用rx_indicate读取一个完整的rt_sensor_data
 * 8）如果传感器工作状态置为FIFO中断模式，则调用rx_indicate读取fifo_max个完整的rt_sensor_data
 *
 * 至此可以这么理解：检测到一次中断，对于RT_SENSOR_MODE_INT工作模式则读取一个sensor_data到传感器的data_buf
 * 对于RT_SENSOR_MODE_FIFO工作模式则读取fifo_max个sensor_data到传感器的data_buf
 * 但是令人不解的是rt_sensor_cb中，在区分RT_SENSOR_MODE_INT和RT_SENSOR_MODE_FIFO之前有如下语句
 * if (sen->data_len > 0)
 *  {
 *     sen->parent.rx_indicate(&sen->parent, sen->data_len / sizeof(struct rt_sensor_data));
 *  }
 *  此语句的含义是当已知data_len>0则读取尽可能多的rt_sensor_data到传感器的data_buf中
 *  有一个问题，传感器在触发中断的时候，MCU也许只是检测到一个中断信号，并不知道data_len是多少
 *  除非传感器自动向MCU输出字节
 *  本文专注于bmi270设备，经过查阅数据手册并没有发现bmi270能够自动输出传感器数据
 *  因此sen->data_len > 0不成立，即根据7）和8）步骤进行
 *
 *  但是需要注意sensor.c中rt_hw_sensor_register函数在注册传感器时，默认rx_indicate=RT_NULL
 *  因此默认情况下，即使发生了中断，也不会调用rx_indicate去读取
 *  要想在RT_SENSOR_MODE_INT或者RT_SENSOR_MODE_FIFO模式下，RT-Thread能够自动读取传感器数据到指定buf
 *  则需要用户自己创建rx_indicate函数。
 *
 *  最后，根据以上分析，结合bmi270这款IMU特性，令其工作在轮询模式下，
 *  但是提供工作在FIFO中断模式选项，FIFO中断对应是FIFO的watermark中断
 *  只要watermark中断发生，且工作在FIFO中断模式，则读取1个完整的FIFO帧
 *  并解析此FIFO帧
 *
 *  本驱动不提供RT_SENSOR_MODE_INT工作选项，但是可以指定某些FEATURE发生时触发中断
 *  然后用户可以遍历中断寄存器去查看哪个中断触发了，例如静止中断或者计步器中断
 *
 *
 */

/*
 * BMI270数据手册对于SPI协议的读写有如下说明：
 *
 * 通过SDO线从设备接收到的第一个字节对应于一个虚拟字节，
 * 第二个字节对应于从指定的寄存器地址读出的值。这意味着，对于一个基本的
 * 读取操作，必须读取两个字节，必须删除第一个字节，并且必须解释第二个字节。
 *
 * 当主机想要向bmi270写入寄存器地址数据时，需注意发送寄存器地址最高位为0
 * 当主机想要从bmi270获取寄存器存储的值时，需注意发送寄存器地址最高位为1
 *
 * 如果想要向bmi270寄存器写入一个字节数据：
 * 1）发送寄存器地址，注意最高位为0
 * 2）写入字节
 *
 * 如果想要从bmi270寄存器读出一个字节数据：
 * 1）发送寄存器地址，注意最高位为1
 * 2）读寄存器2个字节，将第二个读出的字节作为寄存器的内容，忽略第一个字节
 *
 * 如果想要从bmi270寄存器连续读出多个字节数据：
 * 1）发送初始寄存器地址，注意最高位为1
 * 2）保持CSB低，连续读字节，第一个读出的字节无效，将第2个字节视为第一个字节，依此类推
 *
 */

#include <rtthread.h>
#include <rtdbg.h>

#include <sensor_bmi270.h>
#include <bmi270.h>
#include <sensor_common.h>

#include <stm32f4xx_hal.h>

#define DBG_ENABLE
// #define DBG_LEVEL DBG_LOG
// #define DBG_SECTION_NAME "sensor.bosch.bmi270"
#define DBG_COLOR

static uint8_t sense_list[] = {
#ifdef BMI270_USING_ACCE
    BMI2_ACCEL,
#endif
#ifdef BMI270_USING_GYRO
    BMI2_GYRO,
#endif
};

/* bmi270 设备 */
static struct bmi2_dev bmi270_device;

/* bmi270 加速度和陀螺仪传感器 */
static rt_device_t bmi270_acce_dev = RT_NULL;
static rt_device_t bmi270_gyro_dev = RT_NULL;

/* bmi270 加速度和陀螺仪传感器的配置信息 */
static struct bmi2_sens_config acce_init_cfg;
static struct bmi2_sens_config gyro_init_cfg;

#ifndef BMI270_USING_SPI
static I2C_HandleTypeDef bmi270_i2c_handle;

/// @brief 该函数向bmi270官方驱动提供以I2C方式读取寄存器接口
/// @param addr 传感器的I2C协议地址-在SPI协议中无效
/// @param reg 寄存器地址
/// @param data_buf 保存读取寄存器的数据
/// @param data_len 指定读取长度
/// @return 读取成功RT_EOK ,失败 RT_EFAIL
static int8_t bmi270_i2c_read_reg(uint8_t reg_addr, uint8_t *data_buf, uint32_t data_len, void *intf_ptr);

/// @brief 该函数向bmi270官方驱动提供以I2C方式写入寄存器接口
/// @param addr 传感器的I2C协议地址-在SPI协议中无效
/// @param reg 寄存器地址
/// @param data_buf 保存写入寄存器的数据
/// @param data_len 指定写入长度
/// @return 写入成功或者失败
static int8_t bmi270_i2c_write_reg(uint8_t reg_addr, const uint8_t *data_buf, uint32_t data_len, void *intf_ptr);
#else
static SPI_HandleTypeDef bmi270_spi_handle;

/// @brief 该函数向bmi270官方驱动提供以SPI方式读取寄存器接口
/// @param addr 传感器的I2C协议地址-在SPI协议中无效
/// @param reg 寄存器地址
/// @param data_buf 保存读取寄存器的数据
/// @param data_len 指定读取长度
/// @return 读取成功RT_EOK ,失败 RT_EFAIL
static int8_t bmi270_spi_read_reg(uint8_t reg_addr, uint8_t *data_buf, uint32_t data_len, void *intf_ptr);

/// @brief 该函数向bmi270官方驱动提供以SPI方式写入寄存器接口
/// @param addr 传感器的I2C协议地址-在SPI协议中无效
/// @param reg 寄存器地址
/// @param data_buf 保存写入寄存器的数据
/// @param data_len 指定写入长度
/// @return 写入成功或者失败
static int8_t bmi270_spi_write_reg(uint8_t reg_addr, const uint8_t *data_buf, uint32_t data_len, void *intf_ptr);
#endif

/// @brief
/// @return
static int bsp_bmi270_init();

/// @brief 该函数向bmi270官方驱动提供延时接口
/// @param period 延时us时长
/// @return
static void bmi270_delay_us(uint32_t period, void *intf_ptr);

/// @brief 该函数向bmi270官方驱动提供延时接口
/// @param period 延时ms时长
/// @return
static void bmi270_delay_ms(uint32_t period, void *intf_ptr);

/* device interface */
static rt_err_t rt_bmi270_acce_open(rt_device_t dev, rt_uint16_t oflag);
static rt_err_t rt_bmi270_acce_close(rt_device_t dev);
static rt_size_t rt_bmi270_acce_read(rt_device_t dev, rt_off_t pos, void *buffer, rt_size_t size);
static rt_size_t rt_bmi270_acce_write(rt_device_t dev, rt_off_t pos, const void *buffer, rt_size_t size);

static rt_err_t rt_bmi270_control(rt_device_t dev, int cmd, void *args);

static rt_err_t rt_bmi270_gyro_open(rt_device_t dev, rt_uint16_t oflag);
static rt_err_t rt_bmi270_gyro_close(rt_device_t dev);
static rt_size_t rt_bmi270_gyro_read(rt_device_t dev, rt_off_t pos, void *buffer, rt_size_t size);
static rt_size_t rt_bmi270_gyro_write(rt_device_t dev, rt_off_t pos, const void *buffer, rt_size_t size);

/* control interface */

/// @brief
/// @param sensor
/// @param odr
/// @return
static int8_t bmi270_set_odr(rt_device_t sensor, rt_uint16_t odr);

/// @brief
/// @param sensor
/// @param range
/// @return
static int8_t bmi270_set_range(rt_device_t sensor, rt_uint16_t range);

/// @brief
/// @param pwr_mode
/// @return
static int8_t bmi270_set_pwr_mode(rt_uint8_t pwr_mode);

/// @brief
/// @param no_use
/// @return
static int8_t bmi270_self_test();

/* read sensor */

/// @brief normal read bmi270
/// @param sensor acce/gyro
/// @param buf
/// @param len
/// @return
static int8_t bmi270_read_normal(rt_device_t sensor, sensor_data_t *buf, rt_size_t len);

#ifdef BMI270_USING_FIFO
/// @brief fifo read bmi270
/// @param sensor acce/gyro
/// @param buf
/// @param len
/// @return
static int8_t bmi270_read_fifo(rt_device_t sensor, sensor_common_data *buf, rt_size_t len);
#endif

int rt_bmi270_sensor_register(void)
{
    static rt_uint8_t bmi270_dev_register_passed = 0;
    if (bmi270_dev_register_passed != 0)
        return RT_EOK;

    /* construct device */
    bmi270_acce_dev = rt_device_create(RT_Object_Class_Device, 0);
    if (bmi270_acce_dev == RT_NULL)
    {
        rt_kprintf("error happened when create bmi270 accelerator device\n");
        return RT_ENOMEM;
    }

    bmi270_gyro_dev = rt_device_create(RT_Object_Class_Device, 0);
    if (bmi270_gyro_dev == RT_NULL)
    {
        rt_kprintf("error happened when create bmi270 gyroscope device\n");
        return RT_ENOMEM;
    }

    /* set device function pointer */
    bmi270_acce_dev->init = RT_NULL;
    bmi270_acce_dev->open = rt_bmi270_acce_open;
    bmi270_acce_dev->close = rt_bmi270_acce_close;
    bmi270_acce_dev->read = rt_bmi270_acce_read;
    bmi270_acce_dev->write = rt_bmi270_acce_write;
    bmi270_acce_dev->control = rt_bmi270_control;

    bmi270_gyro_dev->init = RT_NULL;
    bmi270_gyro_dev->open = rt_bmi270_gyro_open;
    bmi270_gyro_dev->close = rt_bmi270_gyro_close;
    bmi270_gyro_dev->read = rt_bmi270_gyro_read;
    bmi270_gyro_dev->write = rt_bmi270_gyro_write;
    bmi270_gyro_dev->control = rt_bmi270_control;

    /* register device */
    rt_err_t rslt1 = rt_device_register(bmi270_acce_dev, BMI270_ACCE_DEV_NAME, RT_DEVICE_FLAG_RDWR);
    rt_err_t rslt2 = rt_device_register(bmi270_gyro_dev, BMI270_GYRO_DEV_NAME, RT_DEVICE_FLAG_RDWR);
    if (rslt1 != RT_EOK)
    {
        LOG_E("error happened when register bmi270 accelerator device, rslt = %d\n", rslt1);
    }
    if (rslt2 != RT_EOK)
    {
        LOG_E("error happened when register bmi270 gyroscope device, rslt = %d\n", rslt2);
    }

    int rslt = bsp_bmi270_init();
    if (rslt == RT_EOK)
    {
        bmi270_dev_register_passed = 1;
    }

    return rslt;
}

#ifndef BMI270_USING_SPI
/// @brief i2c low level init
/// @return
static int bmi270_i2c_msp_init()
{
    GPIO_InitTypeDef GPIO_InitStruct;

    /*##-1- Enable peripherals Clocks #################################*/
    BMI270_I2C_SCL_GPIO_RCC_ENABLE();
    BMI270_I2C_SDA_GPIO_RCC_ENABLE();
    BMI270_I2C_RCC_ENABLE();

    /*##-2- Configure peripheral GPIO ##########################################*/
    /* UART TX GPIO pin configuration  */
    GPIO_InitStruct.Pin = BMI270_I2C_SCL_PIN;
    GPIO_InitStruct.Mode = GPIO_MODE_AF_OD;
    GPIO_InitStruct.Pull = GPIO_NOPULL;
    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_MEDIUM;
    GPIO_InitStruct.Alternate = BMI270_I2C_SCL_AF;

    HAL_GPIO_Init(BMI270_I2C_SCL_GPIO, &GPIO_InitStruct);

    /* UART RX GPIO pin configuration  */
    GPIO_InitStruct.Pin = BMI270_I2C_SDA_PIN;
    GPIO_InitStruct.Alternate = BMI270_I2C_SDA_AF;

    HAL_GPIO_Init(BMI270_I2C_SDA_GPIO, &GPIO_InitStruct);

    I2C_InitTypeDef I2C_InitStruct;
    I2C_InitStruct.AddressingMode = I2C_ADDRESSINGMODE_7BIT;
    I2C_InitStruct.ClockSpeed = 100 * 1000;
    I2C_InitStruct.DualAddressMode = I2C_DUALADDRESS_DISABLE;
    I2C_InitStruct.DutyCycle = I2C_DUTYCYCLE_2;
    I2C_InitStruct.GeneralCallMode = I2C_GENERALCALL_DISABLE;
    I2C_InitStruct.NoStretchMode = I2C_NOSTRETCH_DISABLE;
    I2C_InitStruct.OwnAddress1 = 0x01;

    BMI270_i2c_handle.Instance = BMI270_I2C;
    BMI270_i2c_handle.Init = I2C_InitStruct;
    BMI270_i2c_handle.MemaddSize = I2C_MEMADD_SIZE_8BIT;

    // HAL_I2CEx_ConfigAnalogFilter(&BMI270_i2c_handle, I2C_ANALOGFILTER_ENABLE);

    if (HAL_I2C_Init(&BMI270_i2c_handle) != HAL_OK)
        return RT_ERROR;

    __HAL_I2C_ENABLE(&BMI270_i2c_handle);

    return RT_EOK;
}

#else
/// @brief spi low level init
/// @return
static int bmi270_spi_msp_init()
{
    GPIO_InitTypeDef GPIO_InitStruct;

    /*##-1- Enable peripherals Clocks #################################*/
    BMI270_SPI_SCK_GPIO_RCC_ENABLE();
    BMI270_SPI_MISO_GPIO_RCC_ENABLE();
    BMI270_SPI_MOSI_GPIO_RCC_ENABLE();
    BMI270_SPI_CS_GPIO_RCC_ENABLE();

    BMI270_SPI_RCC_ENABLE();

    /*##-2- Configure peripheral GPIO ##########################################*/
    GPIO_InitStruct.Pin = BMI270_SPI_SCK_PIN;
    GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
    GPIO_InitStruct.Pull = GPIO_PULLUP;
    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH;
    GPIO_InitStruct.Alternate = BMI270_SPI_SCK_AF;
    HAL_GPIO_Init(BMI270_SPI_SCK_GPIO, &GPIO_InitStruct);

    GPIO_InitStruct.Pin = BMI270_SPI_MOSI_PIN;
    GPIO_InitStruct.Alternate = BMI270_SPI_MOSI_AF;
    HAL_GPIO_Init(BMI270_SPI_MOSI_GPIO, &GPIO_InitStruct);

    GPIO_InitStruct.Pin = BMI270_SPI_MISO_PIN;
    GPIO_InitStruct.Alternate = BMI270_SPI_MISO_AF;
    HAL_GPIO_Init(BMI270_SPI_MISO_GPIO, &GPIO_InitStruct);

    GPIO_InitStruct.Pin = BMI270_SPI_CS_PIN;
    GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
    HAL_GPIO_Init(BMI270_SPI_CS_GPIO, &GPIO_InitStruct);

    bmi270_spi_handle.Instance = BMI270_SPI;
    bmi270_spi_handle.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_8;
    bmi270_spi_handle.Init.Direction = SPI_DIRECTION_2LINES;
    bmi270_spi_handle.Init.CLKPhase = SPI_PHASE_1EDGE;
    bmi270_spi_handle.Init.CLKPolarity = SPI_POLARITY_LOW;
    bmi270_spi_handle.Init.CRCCalculation = SPI_CRCCALCULATION_DISABLE;
    bmi270_spi_handle.Init.CRCPolynomial = 7;
    bmi270_spi_handle.Init.DataSize = SPI_DATASIZE_8BIT;
    bmi270_spi_handle.Init.FirstBit = SPI_FIRSTBIT_MSB;
    bmi270_spi_handle.Init.NSS = SPI_NSS_SOFT;
    bmi270_spi_handle.Init.TIMode = SPI_TIMODE_DISABLE;

    bmi270_spi_handle.Init.Mode = SPI_MODE_MASTER;

    if (HAL_SPI_Init(&bmi270_spi_handle) != HAL_OK)
    {
        return RT_ERROR;
    }

    __HAL_SPI_ENABLE(&bmi270_spi_handle);

    return RT_EOK;
}
#endif

/// @brief 初始创建bmi270设备时，分别设置加速度和陀螺仪的参数
/// @param acce_init_cfg
/// @param gyro_init_cfg
/// @param hdev
/// @return
static int8_t bmi270_set_acce_gyr_init_config(struct bmi2_dev *hdev)
{
    /* Bosch官方API bmi270_init函数调用了bmi2_write_config_file
       写入bmi270配置文件暂时没有资料表明bmi270官方的配置文件含义
       因此，在bmi270_init之后调用此函数按照预定设计配置bmi270的加速度
       和陀螺仪的ODR、范围等特性
    */
    int8_t rslt = RT_EOK;

    /* 更新加速度配置 */
    acce_init_cfg.type = BMI2_ACCEL;
    rslt = bmi2_get_sensor_config(&acce_init_cfg, 1, &bmi270_device);
    if (rslt != BMI2_OK)
    {
        LOG_E("imu_from_raw_data function: bmi2_get_sensor_config error\n");
        return RT_ERROR;
    }

    acce_init_cfg.cfg.acc.odr = BMI2_ACC_ODR_800HZ;
    acce_init_cfg.cfg.acc.bwp = BMI2_ACC_NORMAL_AVG4;
    acce_init_cfg.cfg.acc.filter_perf = BMI2_ENABLE;
    acce_init_cfg.cfg.acc.range = BMI2_ACC_RANGE_8G; /* 这里设置加速度范围8g，那么参照数据手册分辨率为4096LSB/g */
    rslt = bmi270_set_sensor_config(&acce_init_cfg, 1, hdev);
    if (rslt != RT_EOK)
    {
        LOG_E("bmi270_create--->bmi270_set_sensor_config-->acce failed\n");
    }

    /* 更新陀螺仪配置 */
    gyro_init_cfg.type = BMI2_GYRO;
    rslt = bmi2_get_sensor_config(&gyro_init_cfg, 1, &bmi270_device);
    if (rslt != BMI2_OK)
    {
        LOG_E("imu_from_raw_data function: bmi2_get_sensor_config error\n");
        return RT_ERROR;
    }

    gyro_init_cfg.cfg.gyr.bwp = BMI2_GYR_NORMAL_MODE;
    gyro_init_cfg.cfg.gyr.filter_perf = BMI2_ENABLE;
    gyro_init_cfg.cfg.gyr.odr = BMI2_GYR_ODR_800HZ;
    gyro_init_cfg.cfg.gyr.noise_perf = BMI2_ENABLE;
    gyro_init_cfg.cfg.gyr.ois_range = BMI2_GYR_OIS_250;
    gyro_init_cfg.cfg.gyr.range = BMI2_GYR_RANGE_1000; /* 这里设置陀螺仪范围1000dps，那么参照数据手册分辨率为32.768LSB/dps */
    rslt = bmi270_set_sensor_config(&gyro_init_cfg, 1, hdev);
    if (rslt != RT_EOK)
    {
        LOG_E("bmi270_create--->bmi270_set_sensor_config-->gyro failed\n");
    }

    return rslt;
}

static int bsp_bmi270_init()
{
    bmi270_device.delay_us = bmi270_delay_us;
    bmi270_device.chip_id = 0;

#ifdef BMI270_USING_SPI
    bmi270_device.intf = BMI2_SPI_INTF;
    bmi270_device.read = bmi270_spi_read_reg;
    bmi270_device.write = bmi270_spi_write_reg;
    bmi270_spi_msp_init();
#else
    bmi270_device.intf = BMI2_I2C_INTF;
    bmi270_device.read = bmi270_i2c_read_reg;
    bmi270_device.write = bmi270_i2c_write_reg;
    bmi270_i2c_msp_init();
#endif

    /* initialize bmi270 device */
    int rslt = bmi270_init(&bmi270_device);
    if (rslt != BMI2_OK)
    {
        rt_kprintf("bmi270 init failed, chip id read 0x%02x, which expect 0x%02x\n", bmi270_device.chip_id, BMI270_CHIP_ID);
        return RT_ERROR;
    }
    rt_kprintf("bmi270 init success, chip id read 0x%02x is correct\n", bmi270_device.chip_id);

    /* enable acce and gyro sensor, set power mode */
    int cmd = BMI270_POWER_NORMAL;
    rt_bmi270_control(bmi270_acce_dev, BMI270_CMD_SET_PWR_MODE, &cmd);

    rslt = bmi270_set_acce_gyr_init_config(&bmi270_device);

#ifdef BMI270_USING_FIFO
    /* 调用bmi2_set_fifo_config设置fifo帧格式 */
    /* 这里重点是FIFO帧有帧头，以及AUX、加速度、陀螺仪数据 */
    uint16_t fifo_config = 0;
    fifo_config |= FIFO_HAS_HEAD;
#ifdef BMI270_USING_ACCE
    fifo_config |= FIFO_HAS_ACCE;
#endif
#ifdef BMI270_USING_GYRO
    fifo_config |= FIFO_HAS_GYRO;
#endif
    /* 参见bmi270数据手册 */
    rslt = bmi2_set_fifo_config(fifo_config, BMI2_ENABLE, &bmi270_device);
#endif

    return rslt;
}

/* device interface implementation */
static rt_err_t rt_bmi270_acce_open(rt_device_t dev, rt_uint16_t oflag)
{
    if (dev == RT_NULL || dev != bmi270_acce_dev)
    {
        LOG_E("bmi270 accel open failed for device pointer not correct\n");
        return RT_ERROR;
    }

    /* Performing a dummy read to bring interface back to SPI from I2C interface */
    int rslt = 0;
    /* Variable to assign chip id */
    uint8_t chip_id = 0;
    if (bmi270_device.intf == BMI2_SPI_INTF)
    {
        rslt = bmi2_get_regs(BMI2_CHIP_ID_ADDR, &chip_id, 1, &bmi270_device);
    }

    /* Read chip-id of the BMI2 sensor */
    if (rslt == BMI2_OK)
    {
        rslt = bmi2_get_regs(BMI2_CHIP_ID_ADDR, &chip_id, 1, &bmi270_device);
        if (rslt == BMI2_OK && chip_id == bmi270_device.chip_id)
        {
            return RT_EOK;
        }
    }

#ifndef BMI270_USING_SPI
    bmi270_i2c_msp_init();
#else
    bmi270_spi_msp_init();
#endif
    return RT_EOK;
}

static rt_err_t rt_bmi270_acce_close(rt_device_t dev)
{
    if (dev == RT_NULL || dev != bmi270_acce_dev)
    {
        LOG_E("bmi270 accel close failed for device pointer not correct\n");
        return RT_ERROR;
    }

#ifndef BMI270_USING_SPI
    __HAL_I2C_DISABLE(&bmi270_i2c_handle);
    HAL_I2C_DeInit(&bmi270_i2c_handle);
#else
    __HAL_SPI_DISABLE(&bmi270_spi_handle);
    HAL_SPI_DeInit(&bmi270_spi_handle);
#endif

    return RT_EOK;
}

static rt_size_t rt_bmi270_acce_read(rt_device_t dev, rt_off_t pos, void *buffer, rt_size_t size)
{
    if (dev == RT_NULL || dev != bmi270_acce_dev)
    {
        LOG_E("bmi270 accel read device pointer not correct\n");
        return 0;
    }

#ifdef BMI270_USING_FIFO
    bmi270_read_fifo(bmi270_acce_dev, (sensor_common_data *)buffer, size);
#else
    bmi270_read_normal(bmi270_acce_dev, (sensor_data_t *)buffer, size);
    // bmi270_spi_read_reg(BMI2_ACC_X_LSB_ADDR, buffer, 6, 0);
#endif

    return size;
}

static rt_size_t rt_bmi270_acce_write(rt_device_t dev, rt_off_t pos, const void *buffer, rt_size_t size)
{
    if (dev == RT_NULL || dev != bmi270_acce_dev)
    {
        LOG_E("bmi270 accel write device pointer not correct\n");
        return 0;
    }

    // 写寄存器
    bmi270_device.write((uint8_t)pos, (const uint8_t *)buffer, size, RT_NULL);
    return size;
}

static rt_err_t rt_bmi270_control(rt_device_t dev, int cmd, void *args)
{
    rt_uint32_t *val = (rt_uint32_t *)args;
    rt_err_t rslt = RT_EOK;
    switch (cmd)
    {
    case BMI270_CMD_SET_ODR:
        rslt = bmi270_set_odr(dev, *val);
        break;

    case BMI270_CMD_SET_PWR_MODE:
        rslt = bmi270_set_pwr_mode(*val);
        break;

    case BMI270_CMD_SET_RANGE:
        rslt = bmi270_set_range(dev, *val);

    case BMI270_SELF_TEST:
        rslt = bmi270_self_test();

    default:
        break;
    }

    return rslt;
}

static rt_err_t rt_bmi270_gyro_open(rt_device_t dev, rt_uint16_t oflag)
{
    if (dev == RT_NULL || dev != bmi270_gyro_dev)
    {
        LOG_E("bmi270 gyroscope open failed for device pointer not correct\n");
        return RT_ERROR;
    }

    /* Performing a dummy read to bring interface back to SPI from I2C interface */
    int rslt = 0;
    /* Variable to assign chip id */
    uint8_t chip_id = 0;
    if (bmi270_device.intf == BMI2_SPI_INTF)
    {
        rslt = bmi2_get_regs(BMI2_CHIP_ID_ADDR, &chip_id, 1, &bmi270_device);
    }

    /* Read chip-id of the BMI2 sensor */
    if (rslt == BMI2_OK)
    {
        rslt = bmi2_get_regs(BMI2_CHIP_ID_ADDR, &chip_id, 1, &bmi270_device);
        if (rslt == BMI2_OK && chip_id == bmi270_device.chip_id)
        {
            return RT_EOK;
        }
    }

#ifndef BMI270_USING_SPI
    bmi270_i2c_msp_init();
#else
    bmi270_spi_msp_init();
#endif
    return RT_EOK;
}

static rt_err_t rt_bmi270_gyro_close(rt_device_t dev)
{
    if (dev == RT_NULL || dev != bmi270_gyro_dev)
    {
        LOG_E("bmi270 accel close failed for device pointer not correct\n");
        return RT_ERROR;
    }

#ifndef BMI270_USING_SPI
    __HAL_I2C_DISABLE(&bmi270_i2c_handle);
    HAL_I2C_DeInit(&bmi270_i2c_handle);
#else
    __HAL_SPI_DISABLE(&bmi270_spi_handle);
    HAL_SPI_DeInit(&bmi270_spi_handle);
#endif

    return RT_EOK;
}

static rt_size_t rt_bmi270_gyro_read(rt_device_t dev, rt_off_t pos, void *buffer, rt_size_t size)
{
    if (dev == RT_NULL || dev != bmi270_gyro_dev)
    {
        LOG_E("bmi270 accel read device pointer not correct\n");
        return 0;
    }

#ifdef BMI270_USING_FIFO
    bmi270_read_fifo(bmi270_gyro_dev, (sensor_common_data *)buffer, size);
#else
    bmi270_read_normal(bmi270_gyro_dev, (sensor_data_t *)buffer, size);
#endif

    return size;
}

static rt_size_t rt_bmi270_gyro_write(rt_device_t dev, rt_off_t pos, const void *buffer, rt_size_t size)
{
    if (dev == RT_NULL || dev != bmi270_gyro_dev)
    {
        LOG_E("bmi270 gyroscope write device pointer not correct\n");
        return 0;
    }

    // 写寄存器
    bmi270_device.write((uint8_t)pos, (const uint8_t *)buffer, size, RT_NULL);
    return size;
}

static void bmi270_delay_us(uint32_t period, void *intf_ptr)
{
    if (period <= 1000)
    {
        bmi270_delay_ms(1, intf_ptr);
    }
    else
    {
        bmi270_delay_ms(period / 1000, intf_ptr);
    }
}

static void bmi270_delay_ms(uint32_t period, void *intf_ptr)
{
    rt_thread_mdelay(period);
    // HAL_Delay(period);
}

#ifdef BMI270_USING_SPI
static int8_t bmi270_spi_read_reg(uint8_t reg_addr, uint8_t *data_buf, uint32_t data_len, void *intf_ptr)
{
    // 使用SPI总线，先向BMI270发送寄存器地址，然后读取数据
    HAL_GPIO_WritePin(BMI270_SPI_CS_GPIO, BMI270_SPI_CS_PIN, RESET);
    HAL_SPI_Transmit(&bmi270_spi_handle, &reg_addr, 1, 1);
    HAL_StatusTypeDef rslt = HAL_SPI_Receive(&bmi270_spi_handle, data_buf, data_len, data_len * 5);
    HAL_GPIO_WritePin(BMI270_SPI_CS_GPIO, BMI270_SPI_CS_PIN, SET);
    return rslt == HAL_OK ? RT_EOK : RT_ERROR;
}

static int8_t bmi270_spi_write_reg(uint8_t reg_addr, const uint8_t *data_buf, uint32_t data_len, void *intf_ptr)
{
    // 使用SPI总线，先向BMI270发送寄存器地址，然后向寄存器写入数据
    HAL_GPIO_WritePin(BMI270_SPI_CS_GPIO, BMI270_SPI_CS_PIN, RESET);
    HAL_SPI_Transmit(&bmi270_spi_handle, &reg_addr, 1, 10);
    HAL_StatusTypeDef rslt = HAL_SPI_Transmit(&bmi270_spi_handle, (uint8_t *)data_buf, data_len, data_len * 50);
    HAL_GPIO_WritePin(BMI270_SPI_CS_GPIO, BMI270_SPI_CS_PIN, SET);
    return rslt == HAL_OK ? RT_EOK : RT_ERROR;
}
#else

/* uint8_t reg_addr, uint8_t *reg_data, uint32_t len, void *intf_ptr */
static int8_t bmi270_i2c_read_reg(uint8_t reg_addr, uint8_t *data_buf, uint32_t data_len, void *intf_ptr)
{
    return RT_EOK;
}

static int8_t bmi270_i2c_write_reg(uint8_t reg_addr, const uint8_t *data_buf, uint32_t data_len, void *intf_ptr)
{
    return RT_EOK;
}
#endif

#ifdef BMI270_USING_FIFO
static int8_t bmi270_read_fifo(rt_device_t sensor, sensor_common_data *buf, rt_size_t len)
{
    int8_t rslt = RT_EOK;

    struct bmi2_fifo_frame bmi270_fifo_frame;
    struct bmi2_sens_axes_data sens_fifo_data;

    rslt = bmi2_read_fifo_data(&bmi270_fifo_frame, &bmi270_device);
    if (rslt != RT_EOK)
    {
        LOG_E("bmi270 read fifo data failed\n");
        return rslt;
    }

    /* 加速度传感器 */
    if (sensor == bmi270_acce_dev)
    {
        rslt = bmi2_extract_accel(&sens_fifo_data, (uint16_t *)&len, &bmi270_fifo_frame, &bmi270_device);
    }
    /* 陀螺仪 */
    else if (sensor == bmi270_gyro_dev)
    {
        rslt = bmi2_extract_gyro(&sens_fifo_data, (uint16_t *)&len, &bmi270_fifo_frame, &bmi270_device);
    }

    return rslt;
}
#endif

static int8_t bmi270_read_normal(rt_device_t sensor, sensor_data_t *sensor_raw_data, rt_size_t len)
{
    int8_t rslt = RT_EOK;

    struct bmi2_sens_data bmi270_sensor_data;

    /* 调用此函数获得加速度、陀螺仪、AUX、sensortime数据 */
    rslt = bmi2_get_sensor_data(&bmi270_sensor_data, &bmi270_device);
    if (rslt != BMI2_OK)
    {
        LOG_E("bmi270_read-->bmi2_get_sensor_data failed\n");
        return rslt;
    }

    if (sensor == bmi270_acce_dev)
    {
        uint8_t acce_range = acce_init_cfg.cfg.acc.range;
        uint16_t acce_resolution = 0;
        // 根据数据手册中range和分辨率的关系得到分辨率
        if (acce_range == BMI2_ACC_RANGE_2G)
            acce_resolution = BMI270_ACCE_2G_RESOLUTION;

        else if (acce_range == BMI2_ACC_RANGE_4G)
            acce_resolution = BMI270_ACCE_4G_RESOLUTION;

        else if (acce_range == BMI2_ACC_RANGE_8G)
            acce_resolution = BMI270_ACCE_8G_RESOLUTION;

        else
            acce_resolution = BMI270_ACCE_16G_RESOLUTION;

        /* sensor_raw_data->data.acce存储加速度单位统一为m/s^2 */
        sensor_raw_data->data.acce.x = GRAVITY * bmi270_sensor_data.acc.x / acce_resolution;
        sensor_raw_data->data.acce.y = GRAVITY * bmi270_sensor_data.acc.y / acce_resolution;
        sensor_raw_data->data.acce.z = GRAVITY * bmi270_sensor_data.acc.z / acce_resolution;
    }
    else if (sensor == bmi270_gyro_dev)
    {
        uint8_t gyro_range = gyro_init_cfg.cfg.gyr.range;
        float gyro_resolution = 0;
        // 根据数据手册中range和分辨率的关系得到分辨率
        if (gyro_range == BMI2_GYR_RANGE_125)
            gyro_resolution = BMI270_GYRO_125DPS_RESOLUTION;

        else if (gyro_range == BMI2_GYR_RANGE_250)
            gyro_resolution = BMI270_GYRO_250DPS_RESOLUTION;

        else if (gyro_range == BMI2_GYR_RANGE_500)
            gyro_resolution = BMI270_GYRO_500DPS_RESOLUTION;

        else if (gyro_range == BMI2_GYR_RANGE_1000)
            gyro_resolution = BMI270_GYRO_1000DPS_RESOLUTION;

        else
            gyro_resolution = BMI270_GYRO_2000DPS_RESOLUTION;

        /* sensor_raw_data->data.gyro存储角速度单位统一为rad/s */
        sensor_raw_data->data.gyro.x = 1.0f * bmi270_sensor_data.gyr.x / gyro_resolution / RAD2DEG;
        sensor_raw_data->data.gyro.y = 1.0f * bmi270_sensor_data.gyr.y / gyro_resolution / RAD2DEG;
        sensor_raw_data->data.gyro.z = 1.0f * bmi270_sensor_data.gyr.z / gyro_resolution / RAD2DEG;
    }

    return BMI2_OK;
}

/// @brief 设置加速度或陀螺仪输出频率
/// @param sensor
/// @param odr
/// @return
static int8_t bmi270_set_odr(rt_device_t sensor, rt_uint16_t odr)
{
    /* 调用 bmi270_set_sensor_config 改变ODR */
    int8_t rslt = RT_EOK;

    if (sensor == bmi270_acce_dev)
    {
        bmi270_get_sensor_config(&acce_init_cfg, 1, &bmi270_device);
        acce_init_cfg.cfg.acc.odr = odr;
        bmi270_set_sensor_config(&acce_init_cfg, 1, &bmi270_device);
    }
    else if (sensor == bmi270_gyro_dev)
    {
        bmi270_get_sensor_config(&gyro_init_cfg, 1, &bmi270_device);
        gyro_init_cfg.cfg.gyr.odr = odr;
        bmi270_set_sensor_config(&gyro_init_cfg, 1, &bmi270_device);
    }

    return rslt;
}

static int8_t bmi270_set_range(rt_device_t sensor, rt_uint16_t range)
{
    /* 调用 bmi270_set_sensor_config 改变range */

    int8_t rslt = RT_EOK;

    if (sensor == bmi270_acce_dev)
    {
        bmi270_get_sensor_config(&acce_init_cfg, 1, &bmi270_device);
        acce_init_cfg.cfg.acc.range = range;
        bmi270_set_sensor_config(&acce_init_cfg, 1, &bmi270_device);
    }
    else if (sensor == bmi270_gyro_dev)
    {
        bmi270_get_sensor_config(&gyro_init_cfg, 1, &bmi270_device);
        gyro_init_cfg.cfg.gyr.range = range;
        bmi270_set_sensor_config(&gyro_init_cfg, 1, &bmi270_device);
    }

    return rslt;
}

static int8_t bmi270_set_pwr_mode(rt_uint8_t pwr_mode)
{
    /* bmi270_sensor_enable 通过使能指定的传感器修改供电模式 */

    int8_t rslt = RT_EOK;

    switch (pwr_mode)
    {
    case BMI270_POWER_DOWN:
        rslt = bmi270_sensor_disable(sense_list, sizeof(sense_list), &bmi270_device);
        break;

    case BMI270_POWER_NORMAL:
        rslt = bmi270_sensor_enable(sense_list, sizeof(sense_list), &bmi270_device);
        rslt = bmi2_set_adv_power_save(BMI2_DISABLE, &bmi270_device);
        break;

    case BMI270_POWER_LOW:
        rslt = bmi270_sensor_enable(sense_list, sizeof(sense_list), &bmi270_device);
        rslt = bmi2_set_adv_power_save(BMI2_ENABLE, &bmi270_device);
        break;

    default:
        break;
    }

    return rslt;
}

static int8_t bmi270_self_test()
{
    /* bmi2_perform_accel_self_test进行加速度自检 */
    /* bmi2_do_gyro_st进行陀螺仪自检 */
    int8_t rslt1 = RT_EOK;
    int8_t rslt2 = RT_EOK;

    rslt1 = bmi2_perform_accel_self_test(&bmi270_device);
    rslt2 = bmi2_do_gyro_st(&bmi270_device);
    if (rslt1 != RT_EOK)
    {
        rt_kprintf("bmi270_self_test acce failed\n");
        return RT_ERROR;
    }

    if (rslt2 != RT_EOK)
    {
        rt_kprintf("bmi270_self_test gyro failed\n");
        return RT_ERROR;
    }

    rt_kprintf("bmi270_self_test success\n");
    return RT_EOK;
}

static void bmi270_sensor_sample(void)
{
    rt_device_t dev = RT_NULL;
    sensor_data_t data;
    rt_size_t i;

    /* read bmi270 device acce sensor */
    dev = rt_device_find(BMI270_ACCE_DEV_NAME);
    if (dev == RT_NULL)
    {
        rt_kprintf("Can't find %s\n", BMI270_ACCE_DEV_NAME);
        return;
    }

    for (i = 0; i < 5; i++)
    {
        /* 从传感器读取一个数据 */
        rt_device_read(dev, 0, &data, 1);
        rt_kprintf("acce raw data: [%d, %d, %d]\n", data.data.acce.x, data.data.acce.y, data.data.acce.z);
        rt_thread_mdelay(100);
    }

    /* read bmi270 device gyro sensor */
    dev = rt_device_find(BMI270_GYRO_DEV_NAME);
    if (dev == RT_NULL)
    {
        rt_kprintf("Can't find %s\n", BMI270_GYRO_DEV_NAME);
        return;
    }

    for (i = 0; i < 5; i++)
    {
        /* 从传感器读取一个数据 */
        rt_device_read(dev, 0, &data, 1);
        rt_kprintf("gyro raw data: [%d, %d, %d]\n", data.data.gyro.x, data.data.gyro.y, data.data.gyro.z);
        rt_thread_mdelay(100);
    }
}
MSH_CMD_EXPORT(bmi270_sensor_sample, "bmi270_device_sample");

int bmi270_read_id(void)
{
    uint8_t buf[2];
    bmi270_spi_read_reg(BMI2_CHIP_ID_ADDR | BMI2_SPI_RD_MASK, buf, 2, NULL);
    rt_kprintf("read id : %02x %02x\n", buf[0], buf[1]);
    return 0;
}
MSH_CMD_EXPORT(bmi270_read_id, "read bmi270 id");
